#!/usr/bin/python3
# 摔倒判断
# 代码目标：序列输入传感器数据，判断是否发生摔倒。

import pandas as pd
import numpy as np
from enum import IntEnum


## @状态枚举
class status(IntEnum):
    NORMAL   = 0 # 正常
    FALLING  = 1 # 失重
    PEAK     = 2 # 超重
    LIE      = 3 # 躺平
    FALLEN   = 4 # 摔倒

# STATUS = ['正常', '失重', '超重', '躺平', '摔倒']

# 加速度阈值
falling_threshold = 0.05 # g
peak_threshold = 2.5*2.5 # g

# 时间阈值
normal_duration_threshold = 1 # s    正常
falling_duration_threshold = 0.2 # s 失重
peak_duration_threshold = 0.02 # s   超重
duration_threshold = [
    normal_duration_threshold, 
    falling_duration_threshold,
    peak_duration_threshold,
    0, 0]

class FallDectect:
    def __init__(self) -> None:
        # 初始值为 0
        self.state = status.NORMAL      # 初始状态为正常
        self.s_time = 0                 # 状态时间
        self.old_state = status.NORMAL  # 旧状态
        self.os_time = 0                # 旧状态时间

    def __switch_state(self, new_state, now):
        """
        更新状态，并作摔倒判断
        new_state: 新状态
        now: 更新时间
        """
        if self.state != new_state:            
            duration = round(now-self.s_time, 3) # 状态持续时间
            if duration < duration_threshold[self.state]: # 持续时间不达标 回退旧状态
                self.state= self.old_state
                self.s_time = self.os_time
                # print(f" --> {STATUS[self.state]} with {duration}")
            else:
                if (new_state == status.PEAK and self.state == status.FALLING):
                    # 失重 -> 超重 说明发生摔倒
                    self.state = status.FALLEN
                    self.fallen(now)
                    return True

                # 此处更新状态
                # print(f"> {STATUS[self.state]} continue {duration} s", end = ' -> ')  
                self.old_state = self.state
                self.os_time = self.s_time
                self.state = new_state
                self.s_time = now
                # print(STATUS[self.state]) # 新状态
        return False

    def detect_fall(self, real_data):
        """
        real_data: 实时传感器数据，数据格式 (t, ax, ay, az, gx, gy, gz)
        """
        # now  # 时间
        now, ax, ay, az, gx, gy, gz = real_data # 解析数据
        acc_sum = ax*ax+ay*ay+az*az # 计算和加速度

        if acc_sum < falling_threshold: # 失重状态
            self.__switch_state(status.FALLING, now)
        elif acc_sum > peak_threshold: # 超重状态
            self.__switch_state(status.PEAK, now)
        else: # 未达到阈值 正常状态
            self.__switch_state(status.NORMAL, now)

    def fallen(self,now):
        """
        摔倒时调用
        now: 摔倒时间
        """
        print("!!!! FALLEN in", now)
    
if __name__ == '__main__':
    file_name = 'h3/task2/data/mpu_data_fall.csv' 
    data = pd.read_csv(file_name) # 文件时间序列测试

    f = FallDectect()
    for real_data in data.values:
        f.detect_fall(real_data)

    